//
// Created by fins on 24-10-19.
//
#include "pointcloud/filter_custom.hpp"


Filter_Custom::Filter_Custom()
        : Node("pointcloud_filter_node"),
          tf_buffer_(this->get_clock()),
          tf_listener_(tf_buffer_) {

    config = YAML::LoadFile(YAML_FILE_PATH);
    origin_cloud = config["input_topic"]["points_cloud"].as<std::string>();
    processed_cloud = config["output_topic"]["processed_cloud"].as<std::string>();
    frame_id_robot = config["frame_id"]["robot"].as<std::string>();
    frame_id_lidar = config["frame_id"]["lidar"].as<std::string>();
    use_box_filter = config["box_filter"]["use_box_filter"].as<bool>();
    transform_to_robot_frame = config["transform_to_robot_frame"].as<bool>();

    point_cloud_sub_ =  this -> create_subscription<livox_ros_driver2::msg::CustomMsg>(
            origin_cloud, 10, std::bind(&Filter_Custom::pointcloud_callback, this, std::placeholders::_1));

    processed_point_cloud_pub_ = this->create_publisher<livox_ros_driver2::msg::CustomMsg>(processed_cloud, 10);

    while (not transform_initialized_) {
        try {
            transform_l2r = tf_buffer_.lookupTransform(frame_id_robot, frame_id_lidar, tf2::TimePointZero);
            transform_r2l = tf_buffer_.lookupTransform(frame_id_lidar, frame_id_robot, tf2::TimePointZero);
            transform_initialized_ = true;
        } catch (tf2::TransformException &ex) {
            RCLCPP_WARN(this->get_logger(), "%s", ex.what()); // 如果无法找到变换，返回
            transform_initialized_ = false;
            rclcpp::sleep_for(std::chrono::milliseconds(100));
        }
    }
    cloud_filter_custom.loadParams(YAML_FILE_PATH);
    cloud_filter_custom.set_transform_r2l(transform_r2l);
}

void Filter_Custom::pointcloud_callback(const livox_ros_driver2::msg::CustomMsg::SharedPtr msg) {

    auto start_time = std::chrono::high_resolution_clock::now();
    CustomMsgPtr cloud(new CustomMsg);
    *cloud = fromRosMsg(*msg);

    RCLCPP_INFO(this->get_logger(), "point cloud size: %u", cloud->point_num);
    CustomMsgPtr processed_cloud(new CustomMsg);

    if (transform_to_robot_frame) {
    for ( auto &point : cloud->points) {
        // 将点从 lidar_frame 转换到 robot_base
        geometry_msgs::msg::PointStamped point_stamped;
        point_stamped.header.frame_id = frame_id_lidar;  // 设置原始帧
        point_stamped.header.stamp = msg->header.stamp;  // 使用原始点云的时间戳
        point_stamped.point.x = point.x;
        point_stamped.point.y = point.y;
        point_stamped.point.z = point.z;

        geometry_msgs::msg::PointStamped transformed_point;
        tf2::doTransform(point_stamped, transformed_point, transform_l2r);  // 使用 doTransform 将点从 lidar_frame 转换到 robot_base

        // 更新点的位置
        point.x = transformed_point.point.x;
        point.y = transformed_point.point.y;
        point.z = transformed_point.point.z;
        // 这里的 point 是在 robot_base 坐标系下的坐标
    }}

    if (use_box_filter) {
        cloud_filter_custom.filter(cloud, processed_cloud);
    } else {
        *processed_cloud = *cloud;
    }

    RCLCPP_INFO(this->get_logger(), "processed point cloud size: %u", processed_cloud->point_num);
    livox_ros_driver2::msg::CustomMsg::SharedPtr output_msg(new livox_ros_driver2::msg::CustomMsg);
    *output_msg = toRosMsg(*processed_cloud);

    if (transform_to_robot_frame) {
        output_msg->header.frame_id = frame_id_robot;
    } else {
        output_msg->header.frame_id = frame_id_lidar;
    }
    processed_point_cloud_pub_->publish(*output_msg);
    // 获取结束时间
    auto end_time = std::chrono::high_resolution_clock::now();

    // 计算总运行时间
    auto duration = std::chrono::duration_cast<std::chrono::milliseconds>(end_time - start_time).count();
    RCLCPP_INFO(this->get_logger(), "Point cloud processing took %ld milliseconds", duration);
}
